/*
 * To change this template, choose Tools | Templates
 * and open the template in the editor.
 */
package com.grt192.demos;

import com.grt192.controller.pid.GRTPIDController;
import com.grt192.core.GRTObject;
import com.grt192.spot.actuator.GRTDemoLED;
import com.grt192.spot.controller.PIDDistanceDemoControl;
import com.grt192.spot.mechanism.WallHugger.PIDDistanceDemoMech;
import com.grt192.spot.mechanism.hauntedHouse.StatusLight;
import com.grt192.spot.sensor.GRTSonar;
import com.iaroc.irobot.IRobotCreate;
import com.sun.spot.sensorboard.EDemoBoard;

/**
 * Demos pid control of attempting to be a certain distance from a wall.
 * This is a test for our PID controller.
 * It drives a base with a single mounted sonar to a distance of 10 inches from the front.
 * @author ajc
 */
public class PIDDistanceDriveDemo extends GRTObject {

    public PIDDistanceDriveDemo() {
        StatusLight status = new StatusLight(new GRTDemoLED(0));
        status.rawColor(GRTDemoLED.Color.ORANGE);

        StatusLight mechlight = new StatusLight(new GRTDemoLED(2));
        StatusLight control = new StatusLight(new GRTDemoLED(1));

        //this sonar goes in the very front of the robot
        GRTSonar sonar = GRTSonar.fromPWM(EDemoBoard.D4, 50, "sonar");
//        GRTRelay sonarPower = new GRTRelay(0);
//        try {
//            sonarPower.doCommand(GRTRelay.ON);
//        } catch (ActuatorException ex) {
//            ex.printStackTrace();
//        }

        log("new irobot create...");
//        IRobotCreate irc = new IRobotCreate();
        IRobotCreate irc = null;//pass a null create to dry test.
        log("create complete");
        status.rawColor(GRTDemoLED.Color.GREEN);
        mechlight.rawColor(GRTDemoLED.Color.ORANGE);

        //controllers and mechs
        log("initializing mechs...");
        PIDDistanceDemoMech mech = new PIDDistanceDemoMech(irc, sonar, mechlight);
        mech.setPrinting(true);
        GRTPIDController pid = new GRTPIDController(-25, 0, 0, mech, mech);

        PIDDistanceDemoControl demo = new PIDDistanceDemoControl(mech, pid, control);
//        demo.
        log("starting drive");
        demo.start();
    }
}
